package com.guazi.trackingtest.fsensor;

import android.app.Activity;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.support.annotation.Nullable;
import android.view.View;
import android.widget.Toast;

import com.guazi.trackingtest.PackData;
import com.guazi.trackingtest.R;
import com.guazi.trackingtest.Sender;
import com.guazi.trackingtest.cal.Initialization;
import com.guazi.trackingtest.cal.data.Point3D;
import com.guazi.trackingtest.cal.op.Adjust;
import com.guazi.trackingtest.cal.op.CalculateLAcc;
import com.guazi.trackingtest.cal.op.Calibration;
import com.guazi.trackingtest.cal.op.Correction;
import com.guazi.trackingtest.cal.op.LinearWeightedMovingAverage;
import com.kircherelectronics.fsensor.filter.averaging.MeanFilter;
import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.OrientationFusedKalman;
import com.kircherelectronics.fsensor.util.rotation.RotationUtil;

import java.io.IOException;

/**
 * Created by lqy on 2018/4/2.
 */

public class FSensorActivity extends Activity implements SensorEventListener {

    private SensorManager sensorManager;

    private OrientationFusedKalman orientationKalmanFusion;
    private MeanFilter meanFilter;

    private float[] fusedOrientation = new float[3];
    private float[] acceleration = new float[4];
    private float[] magnetic = new float[3];
    private float[] rotation = new float[3];

    private boolean hasAcceleration = false;
    private boolean hasMagnetic = false;

    private Sender mSender;

    @Override
    protected void onCreate(@Nullable Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_fsensor);

        meanFilter = new MeanFilter();
        orientationKalmanFusion = new OrientationFusedKalman();
        sensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);

        reset(null);

        initJsSender();
    }

    @Override
    public void onResume() {
        super.onResume();

        orientationKalmanFusion.startFusion();

        sensorManager.registerListener(this, sensorManager
                        .getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
                SensorManager.SENSOR_DELAY_GAME);

        // Register for sensor updates.
        sensorManager.registerListener(this, sensorManager
                        .getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
                SensorManager.SENSOR_DELAY_GAME);

        // Register for sensor updates.
        sensorManager.registerListener(this,
                sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
                SensorManager.SENSOR_DELAY_GAME);

    }

    @Override
    protected void onDestroy() {
        super.onDestroy();

        if (orientationKalmanFusion != null) {
            orientationKalmanFusion.stopFusion();
        }

        sensorManager.unregisterListener(this);

        if (mSender != null) {
            mSender.stop();
        }
    }

    @Override
    public void onSensorChanged(SensorEvent event) {
        if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
            if (cal_cnt < cal_len) {
                ++cal_cnt;
                cal_val.x += event.values[0];
                cal_val.y += event.values[1];
                cal_val.z += event.values[2];

                if (cal_cnt == cal_len) {
                    cal_val.x /= cal_len;
                    cal_val.y /= cal_len;
                    cal_val.z /= cal_len;
                    calibration.linearValue.setPoint(cal_val);
                }
            } else {
                // Android reuses events, so you probably want a copy
                System.arraycopy(event.values, 0, acceleration, 0, event.values.length);
                hasAcceleration = true;

                runLinear(new Point3D(event.values[0], event.values[1], event.values[2]), event.timestamp);
            }

        } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
            // Android reuses events, so you probably want a copy
            System.arraycopy(event.values, 0, magnetic, 0, event.values.length);
            hasMagnetic = true;
        } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
            // Android reuses events, so you probably want a copy
            System.arraycopy(event.values, 0, rotation, 0, event.values.length);


            if (hasAcceleration && hasMagnetic) {
                if (!orientationKalmanFusion.isBaseOrientationSet()) {
                    orientationKalmanFusion.setBaseOrientation(RotationUtil.getOrientationQuaternionFromAccelerationMagnetic(acceleration, magnetic));
                } else {
                    fusedOrientation = orientationKalmanFusion.calculateFusedOrientation(rotation, event.timestamp, acceleration, magnetic);
                    fusedOrientation = meanFilter.filter(fusedOrientation);
                    lastRotationAngle.x = fusedOrientation[0];
                    lastRotationAngle.y = fusedOrientation[1];
                    lastRotationAngle.z = fusedOrientation[2];
                }
            }
        }
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {

    }

    public void reset(View view) {

        Initialization.init();
        lastPosition = Initialization.position;
        lastVelocity = Initialization.velocity;
        lastAdjustResult = Initialization.linearValue;
//        lastRotationValue = Initialization.rotationValue;
        lastRotationAngle = Initialization.rotationAngle;
        lastTimeStampLinear = 0;
//        lastTimeStampRotation = 0;

        int ma_len = 6;
        cnt_len = 20;
        count = 0;
        cal_len = 100;
//        cal_cnt = 0;
        cal_val = new Point3D();
//        cal_len_1 = 100;
//        cal_cnt_1 = 0;
//        cal_val_1 = new Point3D();

        calibration = new Calibration();
        lAccX = new LinearWeightedMovingAverage(ma_len);
        lAccY = new LinearWeightedMovingAverage(ma_len);
        lAccZ = new LinearWeightedMovingAverage(ma_len);
//        gyrX = new LinearWeightedMovingAverage(ma_len);
//        gyrY = new LinearWeightedMovingAverage(ma_len);
//        gyrZ = new LinearWeightedMovingAverage(ma_len);

//        rotate_cnt = 0;
//        acc_cnt = 0;
//        calRotate = false;
//        magneticValues = null;
//        accelerometerValues = null;
//        rotateValues = new float[3];
//        accValues = new float[3];
////        mSensorManager.registerListener(mListener, mAcc, SensorManager.SENSOR_DELAY_GAME);
//        mSensorManager.registerListener(mListener, mMag, SensorManager.SENSOR_DELAY_GAME);

        if (orientationKalmanFusion != null) {
            orientationKalmanFusion.reset();
        }

        if (mSender != null) {
            mSender.addPackData(new PackData(1, 1, 0, 0, 0));
        }
    }

    private final static double NS2S = 1000000000.0D;

    private long lastTimeStampLinear;

    private LinearWeightedMovingAverage lAccX;
    private LinearWeightedMovingAverage lAccY;
    private LinearWeightedMovingAverage lAccZ;

    private Point3D flag = new Point3D(0, 0, 0);
    private int count;
    private Point3D lastPosition;
    private Point3D lastVelocity;
    private Point3D lastAdjustResult;
    private Point3D lastRotationAngle;
    private int cnt_len;
    private Calibration calibration;

    private int cal_len;
    private int cal_cnt;
    private Point3D cal_val;

    private void runLinear(Point3D accValue, long timeStamp) {
        if (lastTimeStampLinear == 0) {
            lastTimeStampLinear = timeStamp;
            return;
        }

        double timeInterval = (timeStamp - lastTimeStampLinear) / NS2S;

        Point3D accCorrectionResult = Correction.LinearCorrection(accValue, calibration.linearValue);

        Point3D accAfterFilter = new Point3D();

//        if (Math.abs(accCorrectionResult.x) < 0.1)
//            accCorrectionResult.x = 0;
//        if (Math.abs(accCorrectionResult.y) < 0.1)
//            accCorrectionResult.y = 0;
//        if (Math.abs(accCorrectionResult.z) < 0.1)
//            accCorrectionResult.z = 0;

        lAccX.pushValue(accCorrectionResult.x);
        lAccY.pushValue(accCorrectionResult.y);
        lAccZ.pushValue(accCorrectionResult.z);
        accAfterFilter.x = lAccX.getAverageValue();
        accAfterFilter.y = lAccY.getAverageValue();
        accAfterFilter.z = lAccZ.getAverageValue();

        Point3D adjustResult;
        Point3D velocity = new Point3D();

        if (Math.abs(accAfterFilter.x - flag.x) > 0.1 || Math.abs(accAfterFilter.y - flag.y) > 0.1 || Math.abs(accAfterFilter.z - flag.z) > 0.1) {

            flag.setPoint(accAfterFilter);

            adjustResult = Adjust.adjust(accAfterFilter, lastRotationAngle);
            velocity = CalculateLAcc.calculateVelocity(lastVelocity, lastAdjustResult, adjustResult, timeInterval);
            Point3D computationResult = CalculateLAcc.calculateDisplacement(lastVelocity, velocity, timeInterval);
            Point3D position = CalculateLAcc.calculatePosition(lastPosition, computationResult);
            mSender.addPackData(new PackData(0, 0, position.x, position.y, position.z));

            lastTimeStampLinear = timeStamp;
            lastVelocity = velocity;
            lastAdjustResult = adjustResult;
            lastPosition = position;

            count = 0;
        } else {
            ++count;

            flag.setPoint((flag.x * (count - 1) + accAfterFilter.x) / count,
                    (flag.y * (count - 1) + accAfterFilter.y) / count,
                    (flag.z * (count - 1) + accAfterFilter.z) / count);

            if (count > cnt_len) {

                adjustResult = Adjust.adjust(flag, lastRotationAngle);
                velocity.setPoint(0, 0, 0);
                Point3D computationResult = CalculateLAcc.calculateDisplacement(lastVelocity, velocity, timeInterval);
                Point3D position = CalculateLAcc.calculatePosition(lastPosition, computationResult);

                lastTimeStampLinear = timeStamp;
                lastVelocity = velocity;
                lastAdjustResult = adjustResult;
                lastPosition = position;
            }
        }
    }

    private void initJsSender() {
        if (mSender == null) {
            mSender = new Sender();
            mSender.start();
            Toast.makeText(this, "sender初始化成功", Toast.LENGTH_SHORT).show();
            new Thread(new Runnable() {
                @Override
                public void run() {
                    while (true) {
                        try {
                            mSender.sendDataToJs();
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }
            }).start();
        }
    }
}
